#!/usr/bin/env python

import rospy
from std_msgs.msg import Float64, Float32, String

from sailing_robot.sail_table import SailTable, SailData
import sailing_robot.pid_control as _PID

sail_table_dict = rospy.get_param('sailsettings/table')
sheet_out_to_jibe = rospy.get_param('sailsettings/sheet_out_to_jibe', False)
sail_table = SailTable(sail_table_dict)
sail_data = SailData(sail_table)

def node_publisher():
    pub = rospy.Publisher('sailsheet_normalized', Float32, queue_size=10)
    rospy.init_node('actuator_demand_sail', anonymous=True)

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        sheet_normalized = sail_data.calculate_sheet_setting()
        pub.publish(sheet_normalized)
        rate.sleep()


if __name__ == '__main__':
    try:
        rospy.Subscriber('wind_direction_apparent', Float64, sail_data.update_wind)
        rospy.Subscriber('sailing_state', String, sail_data.update_sailing_state)
        node_publisher()
    except rospy.ROSInterruptException:
        pass
